close all
clear all
clc

%% Definizione dei parametri
m  = 5;     %Kg
km = 5;     %N m^2 A^-2
k0 = 4;     %N m^2
kv = 1;     %V m^-1
v0 = 2;     % V
g = 9.81;   %m s^-2

% condizioni iniziali 
x0=1;
dx0=0;
% varie ---
Ts = 0.01;
%% Punto 2. valore dell'ingresso punto x = [1 0] 
ie = sqrt((m*g*x0^2-k0)/km);
ue=ie;

%% Punto 3. Simulazione
Tfin = 3;
x0_p3 = x0;
dx0_p3 = dx0;
sim( 'lev_p3.mdl',Tfin);
figure(1)
hold on
plot(time,y_p3,'LineWidth',1)
x0_p3 = x0 + 0.01;
dx0_p3 = dx0;
sim( 'lev_p3.mdl',Tfin);
plot(time,y_p3,'LineWidth',2)
grid on
legend('x_{0}=1','x_{0}=1.001')
hold off

%% Punto 4. Linearizzazione
Xe =[1 0]';
ye = kv * x0 + v0; % calcolo dell'uscita di equilibrio, si poteva fare anche cos� [Xe,ue,ye,DX]=trim('Levitatore_dee_mdl',[],ie,[],[],1,[])
[A,B,C,D] = linmod('lev_p4',Xe,ue)

%% Punto 5. Stabilita'
disp('Gli autovalori della matrice di stato risultano:')
E = eig(A)
disp('il sistema risulta instabile')

%% Punto 6. Raggiungibilita' e l'osservabilita'
P1 = ctrb(A,B);
disp ('Matrice di Raggiungibilita P1 ha rango massimo');
disp(P1);
%rank(R)
disp ('sistema raggiungibile');

disp ('Matrice di Osservabilita P2 ha rango massimo');
P2 = obsv(A,C);
disp(P2);
disp ('sistema osservabile');

%% Punto 7. Controllo ottimo a tempo infinito

Q=[1 0;0 1];
R=1;
[S,autov,K] = care(A,B,Q,R);
K=-K;

sim('lev_lin_K_p7',Tfin);

rank(B)
%regola di separazione 
L = -place(A',C', [-2 -3])';
sim('lev_lin_Obs_p7',Tfin);
figure(7)
subplot(2,1,1)
plot(time_p7_1,y_lp7_1)
ylabel('y(t)')
xlabel('time')
legend('Regolazione ottima')
grid on
subplot(2,1,2)
plot(time,y_lp7)
ylabel('y(t)')
xlabel('time')
legend('Regolazione con osservatore dello stato')
grid on

%% Punto 8.  Simulazione del sistema non lineare 
sim('lev_lin_p8',5);
figure(8)
plot(time,y_lp8)
grid on
